#include "system.hpp"
#include "apm32f0xx_rcm.h"
#include "apm32f0xx_gpio.h"
#include "apm32f0xx_usart.h"
#include "stdio.h"
#include "apm32f0xx_misc.h"
#include "FreeRTOS.h"

extern "C" unsigned int SystemCoreClock = 48 * 1000 * 1000;
volatile uint32_t System_delayTick = 0;

void System_clockInit(){
    uint32_t i;
    RCM->CTRL1_B.HSEEN = BIT_SET;
    for (i = 0; i < HSE_STARTUP_TIMEOUT; i++)
    {
        if (RCM->CTRL1_B.HSERDYFLG)
        {
            break;
        }
    }
    if (RCM->CTRL1_B.HSERDYFLG)
    {
        /* Enable Prefetch Buffer */
        FMC->CTRL1_B.PBEN = BIT_SET;
        /* Flash 1 wait state */
        FMC->CTRL1_B.WS = 1;
        /* HCLK = SYSCLK */
        RCM->CFG1_B.AHBPSC = 0X00;
        /* PCLK = HCLK */
        RCM->CFG1_B.APB1PSC = 0X00;
        /* PLL: HSE * 6 */
        RCM->CFG1_B.PLLSRCSEL = 2;
        RCM->CFG1_B.PLLMULCFG = 4;
        /* Enable PLL */
        RCM->CTRL1_B.PLLEN = 1;
        /* Wait PLL Ready */
        while (RCM->CTRL1_B.PLLRDYFLG == BIT_RESET);
        /* Select PLL as system clock source */
        RCM->CFG1_B.SCLKSEL = 2;
        /* Wait till PLL is used as system clock source */
        while (RCM->CFG1_B.SCLKSWSTS != 0x02);
    }

    return ;
}


void System_setPeripheralClockEnable(PeripheralClock_t clock, BOOL status){
    int bus = clock / 32;
    int peripheral_index = clock % 32;
    uint32_t peripheral_bit = (0x01 << peripheral_index);
    switch ( bus )
    {
    case 0:
        /* AHB */
        if( status == TRUE){
            RCM_EnableAHBPeriphClock(peripheral_bit);
        }else {
            RCM_DisableAHBPeriphClock(peripheral_bit);
        }
        break;
    case 1:
        /* APB1 */
        if( status == TRUE){
            RCM_EnableAPB1PeriphClock(peripheral_bit);
        }else {
            RCM_DisableAPB1PeriphClock(peripheral_bit);
        }
    case 2:
        /* APB2 */
        if( status == TRUE){
            RCM_EnableAPB2PeriphClock(peripheral_bit);
        }else {
            RCM_DisableAPB2PeriphClock(peripheral_bit);
        }    
    default:
        break;
    }
    return ;
}

void System_printInit(void){
    System_setPeripheralClockEnable(CLK_GPIOA,TRUE);
    System_setPeripheralClockEnable(CLK_USART2,TRUE);
    
    GPIO_ConfigPinAF(GPIOA, GPIO_PIN_SOURCE_2, GPIO_AF_PIN1);
    GPIO_ConfigPinAF(GPIOA, GPIO_PIN_SOURCE_3, GPIO_AF_PIN1);

    GPIO_Config_T gpio_configuration;
    gpio_configuration.pin = GPIO_PIN_2;
    gpio_configuration.mode = GPIO_MODE_AF;
    gpio_configuration.outtype = GPIO_OUT_TYPE_PP;
    gpio_configuration.speed = GPIO_SPEED_50MHz;
    GPIO_Config(GPIOA,&gpio_configuration);
    gpio_configuration.pin = GPIO_PIN_3;
    GPIO_Config(GPIOA,&gpio_configuration);


    USART_Config_T usart_configuration;
    usart_configuration.baudRate = 115200;
    usart_configuration.wordLength = USART_WORD_LEN_8B;
    usart_configuration.parity = USART_PARITY_NONE;
    usart_configuration.stopBits = USART_STOP_BIT_1;
    usart_configuration.hardwareFlowCtrl = USART_FLOW_CTRL_NONE;
    usart_configuration.mode = USART_MODE_TX_RX;
    USART_Config(USART2,&usart_configuration);

    // USART_EnableInterrupt(USART2,USART_INT_RXBNEIE);
    // NVIC_EnableIRQRequest(USART2_IRQn,0x05);

    USART_Enable(USART2);    

    return ;
}

extern "C"
void System_init(){
    RCM->CTRL1_B.HSIEN = BIT_SET;
    /* Reset SCLKSEL, AHBPSC, APB1PSC, APB2PSC, ADCPSC and COC bits */
    RCM->CFG1 &= (uint32_t)0x08FFB80CU;
    /* Reset HSEEN, CSSEN and PLLEN bits */
    RCM->CTRL1 &= (uint32_t)0xFEF6FFFFU;
    /* Reset HSEBCFG bit */
    RCM->CTRL1_B.HSEBCFG = BIT_RESET;
    /* Reset PLLSRCSEL, PLLHSEPSC, PLLMULCFG bits */
    RCM->CFG1 &= (uint32_t)0xFFC0FFFFU;
    /* Reset PREDIV[3:0] bits */
    RCM->CFG1 &= (uint32_t)0xFFFFFFF0U;
    /* Reset USARTSW[1:0], I2CSW, CECSW and ADCSW bits */
    RCM->CFG3 &= (uint32_t)0xFFFFFEAC;
    /* Reset  HSI14 bit */
    RCM->CTRL2_B.HSI14EN = BIT_RESET;
    /* Disable all interrupts */
    RCM->INT = 0x00000000U;
    SCB->VTOR = FMC_BASE | 0x00;
    System_clockInit();
    System_printInit();
    return;
}


extern "C"
int _write(int fd,char *ptr,int len){
    int n = 0;
    for ( n = 0; n < len; n++ ){
        while ( USART_ReadStatusFlag(USART2,USART_FLAG_TXBE) == RESET );
        USART_TxData(USART2,ptr[n]);

    }
    return len;
}

void *operator new(size_t size){
    return pvPortMalloc(size);
}

void *operator new[](size_t size){
    return pvPortMalloc(size);
}

void operator delete(void *mem){
    return vPortFree(mem);
}

void operator delete[](void *mem){
    return vPortFree(mem);
}
